/**
 * @brief  
 * @author  
 * @date    
 */

#pragma once
/*是一种预处理器指令，通常用于防止头文件被多次包含。这是一种头文件保护机制，
确保在同一个编译单元中，同一个头文件只会被包含一次，以防止重复定义。*/
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>
#include <time.h>
#include <stdio.h>
#include <sys/time.h>
#include <memory>
#include <math.h>
#include<vector>
#include<list>
#include<cmath>
#include<ctime>
#include<utility>
#include<eigen3/Eigen/Eigen>
// #include <OsqpEigen/OsqpEigen.h>
// #include <osqp/osqp.h>
#include<std_msgs/msg/header.hpp>
#include<std_msgs/msg/float32.hpp>
#include<std_msgs/msg/int8.hpp>
#include<std_msgs/msg/string.hpp>
/*时间同步*/
#include<message_filters/subscriber.h>
#include<message_filters/time_synchronizer.h>
#include<message_filters/sync_policies/approximate_time.h>

/*User*/
#include"base_interface/msg/trajectory_custom.hpp"
#include"gene_traj/fast_planner.hpp"
#include"base_interface/msg/ops.hpp"
#include"base_interface/msg/cv_data.hpp"
#include"base_interface/msg/hit.hpp"

// 可以使用带单位的时间量 比如下面用的1s 500ms
using namespace std::chrono_literals;
using namespace Eigen;

#define delt_t 0.005
#define PI 3.1415926
// #define PointNumber 3

class Run_Traj_Gene : public rclcpp::Node 
{
public:
    Run_Traj_Gene();
    ~Run_Traj_Gene(){};


private:
    void OpsCallback(const base_interface::msg::Ops::ConstPtr msg);

    void CvCallback(const base_interface::msg::CvData::SharedPtr cv_data);

    void HitMessageCallback(const base_interface::msg::Hit::SharedPtr hit_msg);

    void TimeCallback();

    void MincoTest();
    void acc_max_Test();

    rclcpp::Time base_time_;
    rclcpp::Time ops_stamp;
    rclcpp::Time ops_stamp_last;

    rclcpp::Publisher<base_interface::msg::TrajectoryCustom>::SharedPtr traj_Pub_Custom;
    rclcpp::Subscription<base_interface::msg::Ops>::SharedPtr ops_sub_;
    rclcpp::Subscription<base_interface::msg::CvData>::SharedPtr cv_sub_;
    rclcpp::Subscription<base_interface::msg::Hit>::SharedPtr hit_sub;
    rclcpp::TimerBase::SharedPtr timer_;





};
/**
 * 计算击球平台的中心点相对于原点的位置-----------------------------------------------------------------------------------------------------------------
*/
Eigen::VectorXd The_position_of_the_batting_platform(float angle_pitch, float angle_roll, float angle_yaw);

float acc_compute(float pos_angle, float a_min);

MatrixXd acc_max_trajectory(float total_time, float x_current, float y_current, float x_start_vel, float y_start_vel, float x_target, float y_target, float x_des_vel, float y_des_vel);

MatrixXd sintrajectory(float total_time, float x_current, float y_current, float x_start_vel, float y_start_vel, float x_target, float y_target, float x_des_vel, float y_des_vel);

MatrixXd cubicSpline(float total_time, float x_current, float y_current, float x_start_vel, float y_start_vel, float x_target, float y_target, float x_des_vel, float y_des_vel);

// float NumDecimal(float value,int n_);